#include "drv_ROM.h"
#include "bsp.h"
#ifdef USING_F4_MCU
#include "stm32f4xx_hal_flash_ex.h"
#include "stm32f4xx_hal_flash.h"
#endif
#ifdef USING_L1_MCU
#include "stm32l0xx_hal_flash_ex.h"
#include "stm32l0xx_hal_flash.h"
#endif
#include "cli.h"

// FLASH  sector start up address
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000)  // 16 Kbytes
#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000)  // 16 Kbytes
#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000)  // 16 Kbytes
#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000)  // 16 Kbytes
#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000)  // 64 Kbytes
#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000)  // 128 Kbytes
#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08040000)  // 128 Kbytes
#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x08060000)  // 128 Kbytes
#define ADDR_FLASH_SECTOR_8 ((uint32_t)0x08080000)  // 128 Kbytes
#define ADDR_FLASH_SECTOR_9 ((uint32_t)0x080A0000)  // 128 Kbytes
#define ADDR_FLASH_SECTOR_10 ((uint32_t)0x080C0000) //,128 Kbytes
#define ADDR_FLASH_SECTOR_11 ((uint32_t)0x080E0000) //,128 Kbytes

#define ADDR_FLASH_SECTOR_12 ((uint32_t)0x08100000) //, 16 Kbytes
#define ADDR_FLASH_SECTOR_13 ((uint32_t)0x08104000) //, 16 Kbytes
#define ADDR_FLASH_SECTOR_14 ((uint32_t)0x08108000) //, 16 Kbytes
#define ADDR_FLASH_SECTOR_15 ((uint32_t)0x0810C000) //, 16 Kbytes
#define ADDR_FLASH_SECTOR_16 ((uint32_t)0x08110000) //, 64 Kbytes
#define ADDR_FLASH_SECTOR_17 ((uint32_t)0x08120000) //, 128 Kbytes
#define ADDR_FLASH_SECTOR_18 ((uint32_t)0x08140000) //, 128 Kbytes
#define ADDR_FLASH_SECTOR_19 ((uint32_t)0x08160000) //, 128 Kbytes
#define ADDR_FLASH_SECTOR_20 ((uint32_t)0x08180000) //, 128 Kbytes
#define ADDR_FLASH_SECTOR_21 ((uint32_t)0x081A0000) //, 128 Kbytes
#define ADDR_FLASH_SECTOR_22 ((uint32_t)0x081C0000) //, 128 Kbytes
#define ADDR_FLASH_SECTOR_23 ((uint32_t)0x081E0000) //, 128 Kbytes

uint32_t addr2sector(uint32_t addr)
{
    if (addr < ADDR_FLASH_SECTOR_1)
        return FLASH_SECTOR_0;
    else if (addr < ADDR_FLASH_SECTOR_2)
        return FLASH_SECTOR_1;
    else if (addr < ADDR_FLASH_SECTOR_3)
        return FLASH_SECTOR_2;
    else if (addr < ADDR_FLASH_SECTOR_4)
        return FLASH_SECTOR_3;
    else if (addr < ADDR_FLASH_SECTOR_5)
        return FLASH_SECTOR_4;
    else if (addr < ADDR_FLASH_SECTOR_6)
        return FLASH_SECTOR_5;
    else if (addr < ADDR_FLASH_SECTOR_7)
        return FLASH_SECTOR_6;
    else if (addr < ADDR_FLASH_SECTOR_8)
        return FLASH_SECTOR_7;
    else
        return FLASH_SECTOR_7;
}

void ROM_write(uint32_t addr, uint32_t *src, uint32_t len)
{
    FLASH_EraseInitTypeDef FlashEraseInit;
    HAL_StatusTypeDef FlashStatus = HAL_OK;
    uint32_t SectorError = 0;
    uint32_t addrx = 0;
    uint32_t endaddr = 0;
    uint32_t *pBuffer = src;
    if ((addr >= PARA_WRITE_ADDR + PARA_WRITE_MAX) ||
        (addr + len * sizeof(uint32_t) >= PARA_WRITE_ADDR + PARA_WRITE_MAX))
    {
        return;
    }

    HAL_FLASH_Unlock();
    addrx = addr;
    endaddr = addr + len * 4;

    /* 擦除扇区 */
    if (addrx < 0X1FFF0000)
    {
        while (addrx < endaddr)
        {
            if (*(__IO uint32_t *)addrx != 0XFFFFFFFF)
            {
                FlashEraseInit.TypeErase = FLASH_TYPEERASE_SECTORS;
                FlashEraseInit.Sector = addr2sector(addrx);
                FlashEraseInit.NbSectors = 1;
                FlashEraseInit.VoltageRange = FLASH_VOLTAGE_RANGE_3;
                if (HAL_FLASHEx_Erase(&FlashEraseInit, &SectorError) != HAL_OK)
                {
                    break;
                }
            }
            else
                addrx += 4;
            FLASH_WaitForLastOperation(PARA_WRITE_TIMEOUT);
        }
    }
    /* 写入数据 */
    FlashStatus = FLASH_WaitForLastOperation(PARA_WRITE_TIMEOUT);
    if (FlashStatus == HAL_OK)
    {
        while (addr < endaddr)
        {
            if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, *pBuffer) != HAL_OK)
            {
                break;
            }
            addr += 4;
            pBuffer++;
        }
    }
    HAL_FLASH_Lock();
}

void ROM_read(uint32_t addr, uint32_t *dst, uint32_t len)
{
    if ((addr >= PARA_WRITE_ADDR + PARA_WRITE_MAX) ||
        (addr + len * sizeof(uint32_t) >= PARA_WRITE_ADDR + PARA_WRITE_MAX))
    {
        return;
    }
    for (uint32_t i = 0; i < len; i++)
    {
        dst[i] = *(__IO uint32_t *)addr;
        addr += 4;
    }
}


void saveFile(char *str, int len)
{
  uint32_t *pFile = (uint32_t *)algoLinearCalib_GetParaTable();
  uint32_t fileLen = sizeof(algoLinearCalibParaTable);

  ROM_write(PARA_WRITE_ADDR, pFile, fileLen);
  LOG("size = %d wrote to addr 0x%08x", fileLen, PARA_WRITE_ADDR);
}

void readFlash(char *str, int len)
{
  uint32_t addr, bufLen;
  uint32_t n = sscanf(str, "0x%08x,%d", &addr, &bufLen);
  if (n != 2)
  {
    LOG("sytax error: %s (n=%d)", str, n);
    return;
  }
  uint32_t *buf = (uint32_t *)malloc(bufLen);
  ROM_read(addr, buf, bufLen);
  for (int cnt = 0; cnt < bufLen; cnt++)
  {
    if ((cnt != 0) && (cnt % 8 == 0))
    {
      printf("\r\n");
    }
    printf("0x%08x ", buf[cnt]);
  }
  if (bufLen % 8 == 0)
  {
    printf("\r\n");
  }
  free(buf);
}